#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

;; (setq ros::*compile-message* t)

;; ros init
(ros::roseus "test-openrave-graspplanning")

(ros::roseus-add-msgs "sensor_msgs")
(ros::roseus-add-msgs "object_manipulation_msgs")
(ros::roseus-add-srvs "object_manipulation_msgs")
(ros::roseus-add-srvs "orrosplanning")

(unix:putenv "ROBOT=HRP2JSKNT")
(unix:putenv "HRP2NO=16")

(unless (boundp '*base-frame*)
  (ros::defrosparams
    (*base-frame* "~/frame_id" "/HRP2JSKNT/base_link")
    ))

(load "graspplanning-util.l")
(load "util-cds.l")

(load "irt-all-objects-pointcloud.l")
(load "package://jskpointcloud/src/euslisp/init.l")
(setup-roseus-services)

(defun testgraspplanning (&key (grasparm :rarm) (robot :pr2) (obj-name "petbottle"))
  (set-robot robot)
  (multiple-value-bind (mdl 3dp)
      (make-irt-objects-3dp-from-name obj-name :return-object t)
    (setq *obj* mdl)
    (set-grasp-parameters 3dp robot)
    (set-grasp-parameters-service-call)
    (setq *graspplanning-res*
          (graspplanning-service-call
           grasparm
           (create-GraspableObject-msg-from-pcloud
            3dp "/map")))
    (list mdl 3dp (display-grasp-candidate grasparm robot))
    ))

(defun testgraspplanning-with-trimesh-old
  (model-name &key (grasparm :rarm) (robot :pr2))
  (load (format nil "package://euslisp/jskeus/eus/models/~A-object.l"        
                model-name))
  (let ((mdl (eval
              (read-from-string
               (format nil "(~A)" model-name)))))
    (set-robot robot)
    (multiple-value-bind (3dp idx)
        (eusmodel-to-trimesh-ordered-pcloud mdl :return-eus3dp t)
      (setq *obj* mdl)
      (set-grasp-parameters 3dp robot)
      (set-grasp-parameters-service-call)
      (setq *graspplanning-res*
            (graspplanning-service-call
             grasparm
             (create-GraspableObject-msg-from-pcloud
              3dp "/map" :index-channel idx)))
      (list mdl 3dp (display-grasp-candidate grasparm robot))
      )))

(defun testgraspplanning-with-trimesh
  (model-name &key (grasparm :rarm) (robot :hrp2jsknt))
  (set-robot robot)
  (set-grasp-parameters 3dp robot)
  (set-grasp-parameters-service-call)
  (let* ((mdl (with-open-file (f (format nil "~A/irtmodels/~A-trimesh.l"
                                         (read (piped-fork "rospack find eusmodel2trimesh"))
                                         model-name
                                         ))
                              (read f))))
    (multiple-value-bind (3dp idx)
        (eusmodel-to-trimesh-ordered-pcloud mdl :input-trimesh t)
      (setq *obj* mdl)
      (setq *graspplanning-res*
            (graspplanning-service-call
             grasparm
             (create-GraspableObject-msg-from-pcloud
              3dp "/map" :index-channel idx)))
      (list mdl 3dp (display-grasp-candidate grasparm robot))
      )))

(defun testgraspplanning-irt-all-objects (&key (grasparm :rarm) (robot :hrp2jsknt))
  (set-robot robot)
  (set-grasp-parameters 3dp robot)
  (set-grasp-parameters-service-call)

  (make-irt-all-objects-3dp :use-poisson nil) ;;tmp. :use-poisson t is very slow...
  (dolist (3dp *irt-all-objects-3dp*)
    (let (h-lst)
      (setq *graspplanning-res*
            (graspplanning-service-call
             grasparm
             (create-GraspableObject-msg-from-pcloud
              3dp "/map")))
      (setq h-lst (display-grasp-candidate grasparm robot))
      (objects (list 3dp h-lst))
      (dump-structure
       (format nil "~A-~A-grasps.l"
               (send 3dp :name)
               (send *robot* :name))
       (list 3dp h-lst))
      (send *viewer* :draw-objects)
      (send *viewer* :viewsurface :write-to-ppm-file
            (format nil "~A-~A-grasps.ppm" (send 3dp :name) (send *robot* :name)))
      ))
  )